#include "vehicle_lmpc_base.hpp"

namespace auto_ros
{
namespace control
{

VehicleLMpcBase::VehicleLMpcBase(const std::string dyn_mpc_kf_config_path, const std::string vehicle_lmpc_yaml_name, double rate)
{

	YAML::Node dyn_mpc_kf_yaml = YAML::LoadFile(dyn_mpc_kf_config_path + vehicle_lmpc_yaml_name);
	std::fstream fout(dyn_mpc_kf_config_path + vehicle_lmpc_yaml_name + ".cor", std::ios::out);
	fout << dyn_mpc_kf_yaml;
	fout.close();
	KfFactory kf_factory;
	if (dyn_mpc_kf_yaml["has_kf"].as<int>() == 1)
	{
		kf_lateral_ptr_ = kf_factory.CreateSharedObject(
			dyn_mpc_kf_yaml["kf_class_name"].as<std::string>(),
			dyn_mpc_kf_config_path + dyn_mpc_kf_yaml["kf_yaml_name"].as<std::string>(), rate);

		if (kf_lateral_ptr_ == nullptr)
		{
			std::cout << "\033[31m Error: VehicleLMpcBase::VehicleLMpcBase kf_lateral_ptr_ create failed \033[0m"
					  << std::endl;
			std::cout << dyn_mpc_kf_config_path +
							 dyn_mpc_kf_yaml["kf_yaml_name"].as<std::string>()
					  << std::endl;
			throw 100;
		}
		else
		{
			std::cout << "kf created in lmpc,type is:" << dyn_mpc_kf_yaml["kf_class_name"].as<std::string>() << std::endl
					  << "config yaml file is:" << dyn_mpc_kf_config_path + dyn_mpc_kf_yaml["kf_yaml_name"].as<std::string>()
					  << std::endl;
		}
	}

	VehicleDynFactory dyn_factory;
	std::cout << dyn_mpc_kf_yaml["dyn_type"].as<std::string>() << std::endl;
	vehicle_dyn_ptr_ = dyn_factory.CreateSharedObject(
		dyn_mpc_kf_yaml["dyn_type"].as<std::string>(),
		dyn_mpc_kf_config_path + dyn_mpc_kf_yaml["dyn_yaml_file_name"].as<std::string>());
	if (vehicle_dyn_ptr_ == nullptr)
	{

		std::cout << "\033[31m Error: VehicleLMpcBase::VehicleLMpcBase vehicle_dyn_ptr_ create failed \033[0m"
				  << std::endl;
		throw 100;
	}
	else
	{
		std::cout << "vechile dyn created in lmpc,type is:" << dyn_mpc_kf_yaml["dyn_type"].as<std::string>() << std::endl
				  << "config yaml file is:" << dyn_mpc_kf_config_path + dyn_mpc_kf_yaml["dyn_yaml_file_name"].as<std::string>()
				  << std::endl;
	}
	// define steady state solver and constriants
	ref_state_slover_ptr_ = std::make_shared<LinearSteadyState>(vehicle_dyn_ptr_);
	mpc_yaml_ = YAML::LoadFile(dyn_mpc_kf_config_path + dyn_mpc_kf_yaml["mpc_yaml_file_name"].as<std::string>());
	rate_ = rate;
	x_ref_ = Eigen::VectorXd::Zero(vehicle_dyn_ptr_->state_dim());	 //resize(dis_dyn_ptr_->state_dim());
	u_ref_ = Eigen::VectorXd::Zero(vehicle_dyn_ptr_->control_dim()); //resize(dis_dyn_ptr_->control_dim());
}

YAML::Node VehicleLMpcBase::mpc_yaml()
{
	return mpc_yaml_;
}
bool VehicleLMpcBase::step(const Eigen::VectorXd &x0,
						   const Eigen::VectorXd &u_fb, double kappa, double speed)
{
	std::cerr << "\033[31m not implemented by derived class \033[0m" << std::endl;
}
bool VehicleLMpcBase::step(const Eigen::VectorXd &x0,
						   const Eigen::VectorXd &u_fb, const PlanningRefPoint &ref_point, double speed)
{
	std::cerr << "\033[31m not implemented by derived class \033[0m" << std::endl;
}

bool VehicleLMpcBase::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
						   const PlanningRefPoint &ref_point, double speed,
						   const std::map<std::string, double> other_input)
{
	std::cerr << "\033[31m not implemented by derived class \033[0m" << std::endl;
}
bool VehicleLMpcBase::step(const Eigen::VectorXd &x0,
						   const Eigen::VectorXd &u_fb,
						   const std::vector<PlanningRefPoint> &ref_point,
						   double speed)
{
	std::cerr << "\033[31m not implemented by derived class \033[0m" << std::endl;
}
} // namespace control
} // namespace auto_ros